/*
 * @Author: junhuizhou
 * @Date: 2020-12-30 16:07:33
 * @LastEditor: junhuizhou
 * @LastEditTime: 2021-01-09 10:37:43
 * @Description: header
 * @FilePath: \fzhtq\src\ctltask.c
 */

#include <stdio.h>
#include <math.h>
#include "../include/ctltask.h"
#include "../include/ctllaw.h"
#include "../include/constant.h"
#include "../include/calmatrix.h"
#include "../include/caltransmat.h"

static int zerocnt = 0;
static int lasttmptime[3] = { 0, 0, 0 };
static int flagx = 0;
static int flagy = 0;
static int flagz = 0;
static int flag4 = 0;
static int dampflag = 1;
static int mwstart = 0;
mwheel4 momwheel;

void CtlTask(double euler0[3], double eulerRate0[3], double PS_PTime[10], double WhlSpdDsrd[4], double WhlSpdNow[4])
{
    /*数据清零*/
    for(int i=0; i<10; i++)
    {
        PS_PTime[i] = 0.0;
    }
    /*控制*/
    double theta1[3] = {0.0, 0.0, 0.0};
    if(mwstart == 0)
    {
        TaskMWStart();
    }
    if(dampflag == 0)
    {
        momwheel.mwhl1.whlspdnow = WhlSpdNow[0];
        momwheel.mwhl2.whlspdnow = WhlSpdNow[1];
        momwheel.mwhl3.whlspdnow = WhlSpdNow[2];
        momwheel.mwhl4.whlspdnow = WhlSpdNow[3];
        Task3AixsCtl(euler0, eulerRate0, theta1, PS_PTime);
        WhlSpdDsrd[0] = momwheel.mwhl1.whlspddsrd;
        WhlSpdDsrd[1] = momwheel.mwhl2.whlspddsrd;
        WhlSpdDsrd[2] = momwheel.mwhl3.whlspddsrd;
        WhlSpdDsrd[3] = momwheel.mwhl4.whlspddsrd;
        if(fabs(euler0[0]-theta1[0])<DEG2RAD && fabs(euler0[1]-theta1[1])<DEG2RAD && fabs(euler0[2]-theta1[2])<DEG2RAD)
        {
            zerocnt++;
            if(zerocnt > 5)
            {
                zerocnt = 0;
                printf("zero keep finished\n");
            }
        }
        else
        {
            zerocnt = 0;
        }
    }
    else
    {
        TaskRateDamp(eulerRate0, PS_PTime);
    }
    
}

void TaskRateDamp(double omega[3], double pstime[10])
{
    /* 1&6 -X  2&5 +X */
    /* 3&8 +Z  4&7 -Z */
    /* 9   +Y  10  -Y */
    double rate[3];
    int tmptime;     //ms
    double kp = PS_Kp1;
    double thresw = RateDampThresMin;
    int tkt = Ratio_Ctl2Sim;
    int tmptimeup = EnvSim_T_Time*1e3;
    int cnt = 0;
    rate[0] = omega[0]*RAD2DEG;
    rate[1] = omega[1]*RAD2DEG;
    rate[2] = omega[2]*RAD2DEG;

    /* 控制律pstime = kp*dw */
    /*+X -X*/
    if(fabs(rate[0]) > 10.0)
    {
        kp = PS_Kp1;
    }
    else if(fabs(rate[0]) > 5.0)
    {
        kp = PS_Kp2;
    }
    else
    {
        kp = PS_Kp3;
    }
    if(rate[0] > thresw)
    {
        tmptime = kp*rate[0] / tkt;
        tmptime = tmptime < tmptimeup ? tmptime : tmptimeup;
        pstime[0] = tmptime;
        pstime[5] = tmptime;
        pstime[1] = 0.f;
        pstime[4] = 0.f;
    }
    if(rate[0] < -thresw)
    {
        tmptime = -kp*rate[0] / tkt;
        tmptime = tmptime < tmptimeup ? tmptime : tmptimeup;
        pstime[0] = 0.f;
        pstime[5] = 0.f;
        pstime[1] = tmptime;
        pstime[4] = tmptime;
    }
    if(rate[0]<thresw && rate[0]>-thresw)
    {
        pstime[0] = 0.f;
        pstime[5] = 0.f;
        pstime[1] = 0.f;
        pstime[4] = 0.f;
        cnt++;
    }
    /*+Y -Y*/
    if(fabs(rate[1]) > 10.0)
    {
        kp = PS_Kp1;
    }
    else if(fabs(rate[1]) > 5.0)
    {
        kp = PS_Kp2;
    }
    else
    {
        kp = PS_Kp3;
    }
    if(rate[1] > thresw)
    {
        tmptime = kp*rate[1] / tkt * 2;
        tmptime = tmptime < tmptimeup ? tmptime : tmptimeup;
        pstime[9] = tmptime;
        pstime[8] = 0.f;
    }
    if(rate[1] < -thresw)
    {
        tmptime = -kp*rate[1] / tkt * 2;
        tmptime = tmptime < tmptimeup ? tmptime : tmptimeup;
        pstime[9] = 0.f;
        pstime[8] = tmptime;
    }
    if(rate[1]<thresw && rate[1]>-thresw)
    {
        pstime[9] = 0.f;
        pstime[8] = 0.f;
        cnt++;
    }
    /*+Z -Z*/
    if(fabs(rate[2]) > 10.0)
    {
        kp = PS_Kp1;
    }
    else if(fabs(rate[2]) > 5.0)
    {
        kp = PS_Kp2;
    }
    else
    {
        kp = PS_Kp3;
    }
    if(rate[2] > thresw)
    {
        tmptime = kp*rate[2] / tkt;
        tmptime = tmptime < tmptimeup ? tmptime : tmptimeup;
        pstime[3] = tmptime;
        pstime[6] = tmptime;
        pstime[2] = 0.f;
        pstime[7] = 0.f;
    }
    if(rate[2] < -thresw)
    {
        tmptime = -kp*rate[2] / tkt;
        tmptime = tmptime < tmptimeup ? tmptime : tmptimeup;
        pstime[3] = 0.f;
        pstime[6] = 0.f;
        pstime[2] = tmptime;
        pstime[7] = tmptime;
    }
    if(rate[2]<thresw && rate[2]>-thresw)
    {
        pstime[3] = 0.f;
        pstime[6] = 0.f;
        pstime[2] = 0.f;
        pstime[7] = 0.f;
        cnt++;
    }
    /*速率阻尼完成flag*/
    if(cnt == 3)
    {
        dampflag = 0;
        printf("rate damp finished.\n");
    }
}

void TaskMWStart()
{
    momwheel.mwhl1.whlspdnow = momwheel.mwhl2.whlspdnow = momwheel.mwhl3.whlspdnow = momwheel.mwhl4.whlspdnow = 0.0;
    momwheel.mwhl1.whlangmom = momwheel.mwhl2.whlangmom = momwheel.mwhl3.whlangmom = momwheel.mwhl4.whlangmom = 0.0;
    momwheel.mwhl1.whlspddsrd = momwheel.mwhl2.whlspddsrd = momwheel.mwhl3.whlspddsrd = momwheel.mwhl4.whlspddsrd = 0.0;
    momwheel.mwhl1.whltq = momwheel.mwhl2.whltq = momwheel.mwhl3.whltq = momwheel.mwhl4.whltq = 0.0;
    momwheel.mwhl1.whlinertia = momwheel.mwhl2.whlinertia = MomWheel_Inertia;
    momwheel.mwhl3.whlinertia = momwheel.mwhl4.whlinertia = MomWheel_Inertia;
    mwstart = 1;
}

void Task3AixsCtl(double theta0[3], double omega0[3], double theta1[3], double pstime[10])
{
    /*计算所需控制力矩*/
    double Tq[3];
    CalTqByQuat(theta0, omega0, theta1, Tq);
    /*Tq赋值*/
    momwheel.mwhl1.whltq = Tq[0];
    momwheel.mwhl2.whltq = Tq[1];
    momwheel.mwhl3.whltq = Tq[2];
    /*动量轮参数计算*/
    /*deltaSpd = deltaT*omega' and omega' = Tmw/Imw*/
    double detwhlspd;
    double deltaT = EnvSim_T_Time*Ratio_Ctl2Sim;
    detwhlspd = momwheel.mwhl1.whltq*deltaT/momwheel.mwhl1.whlinertia;
    momwheel.mwhl1.whlspddsrd = momwheel.mwhl1.whlspdnow + detwhlspd;
    if(momwheel.mwhl1.whlspddsrd > MomWheel_Spd_Max) momwheel.mwhl1.whlspddsrd = MomWheel_Spd_Max;
    if(momwheel.mwhl1.whlspddsrd < -MomWheel_Spd_Max) momwheel.mwhl1.whlspddsrd = -MomWheel_Spd_Max;
    detwhlspd = momwheel.mwhl2.whltq*deltaT/momwheel.mwhl2.whlinertia;
    momwheel.mwhl2.whlspddsrd = momwheel.mwhl2.whlspdnow + detwhlspd;
    if(momwheel.mwhl2.whlspddsrd > MomWheel_Spd_Max) momwheel.mwhl2.whlspddsrd = MomWheel_Spd_Max;
    if(momwheel.mwhl2.whlspddsrd < -MomWheel_Spd_Max) momwheel.mwhl2.whlspddsrd = -MomWheel_Spd_Max;
    detwhlspd = momwheel.mwhl3.whltq*deltaT/momwheel.mwhl3.whlinertia;
    momwheel.mwhl3.whlspddsrd = momwheel.mwhl3.whlspdnow + detwhlspd;
    if(momwheel.mwhl3.whlspddsrd > MomWheel_Spd_Max) momwheel.mwhl3.whlspddsrd = MomWheel_Spd_Max;
    if(momwheel.mwhl3.whlspddsrd < -MomWheel_Spd_Max) momwheel.mwhl3.whlspddsrd = -MomWheel_Spd_Max;
    detwhlspd = momwheel.mwhl4.whltq*deltaT/momwheel.mwhl4.whlinertia;
    momwheel.mwhl4.whlspddsrd = momwheel.mwhl4.whlspdnow + detwhlspd;
    if(momwheel.mwhl4.whlspddsrd > MomWheel_Spd_Max) momwheel.mwhl4.whlspddsrd = MomWheel_Spd_Max;
    if(momwheel.mwhl4.whlspddsrd < -MomWheel_Spd_Max) momwheel.mwhl4.whlspddsrd = -MomWheel_Spd_Max;
    /*力矩饱和与力矩卸载(使用喷气推力器)*/
    double errortq;
    /*动量轮1卸载*/
    if((momwheel.mwhl1.whlspdnow > MomWheel_Spd_Up) || (momwheel.mwhl1.whlspdnow < -1.0*MomWheel_Spd_Up))
    {
        flagx = 1;
    }
    if(fabs(momwheel.mwhl1.whlspdnow - MomWheel_Spd0) < 100.0 || fabs(momwheel.mwhl1.whlspdnow + MomWheel_Spd0) < 100.0)
    {
        flagx = 0;
    }
    if(flagx == 1)
    {
        /*Tmw = Imw * omega' and omega' = deltaSpd / deltaT*/
        if(momwheel.mwhl1.whlspdnow < 0)
        {
            errortq = momwheel.mwhl1.whlinertia*(-MomWheel_Spd0 - momwheel.mwhl1.whlspdnow) / deltaT;
        }
        else
        {
            errortq = momwheel.mwhl1.whlinertia*(MomWheel_Spd0 - momwheel.mwhl1.whlspdnow) / deltaT;
        }
        if(errortq > MomWheel_Tq_Max) errortq = MomWheel_Tq_Max;
        if(errortq < -MomWheel_Tq_Max) errortq = -MomWheel_Tq_Max;
        if(errortq > 0)
        {
            pstime[0] = pstime[5] = errortq;
        }
        else
        {
            pstime[1] = pstime[4] = -errortq;
        }
        momwheel.mwhl1.whltq = errortq;
        detwhlspd = momwheel.mwhl1.whltq*deltaT/momwheel.mwhl1.whlinertia;
        momwheel.mwhl1.whlspddsrd = momwheel.mwhl1.whlspdnow + detwhlspd;
    }
    /*动量轮2卸载*/
    if((momwheel.mwhl2.whlspdnow > MomWheel_Spd_Up) || (momwheel.mwhl2.whlspdnow < -1.0*MomWheel_Spd_Up))
    {
        flagy = 1;
    }
    if(fabs(momwheel.mwhl2.whlspdnow - MomWheel_Spd0) < 100.0 || fabs(momwheel.mwhl2.whlspdnow + MomWheel_Spd0) < 100.0)
    {
        flagy = 0;
    }
    if(flagy == 1)
    {
        if(momwheel.mwhl2.whlspdnow < 0)
        {
            errortq = momwheel.mwhl2.whlinertia*(-MomWheel_Spd0 - momwheel.mwhl2.whlspdnow) / deltaT;
        }
        else
        {
            errortq = momwheel.mwhl2.whlinertia*(MomWheel_Spd0 - momwheel.mwhl2.whlspdnow) / deltaT;
        }
        if(errortq > MomWheel_Tq_Max) errortq = MomWheel_Tq_Max;
        if(errortq < -MomWheel_Tq_Max) errortq = -MomWheel_Tq_Max;
        if(errortq > 0)
        {
            pstime[9] = 2.0*errortq;
        }
        else
        {
            pstime[8] = -2.0*errortq;
        }
        momwheel.mwhl2.whltq = errortq;
        detwhlspd = momwheel.mwhl2.whltq*deltaT/momwheel.mwhl2.whlinertia;
        momwheel.mwhl2.whlspddsrd = momwheel.mwhl2.whlspdnow + detwhlspd;
    }
    /*动量轮3卸载*/
    if((momwheel.mwhl3.whlspdnow > MomWheel_Spd_Up) || (momwheel.mwhl3.whlspdnow < -1.0*MomWheel_Spd_Up))
    {
        flagz = 1;
    }
    if(fabs(momwheel.mwhl3.whlspdnow - MomWheel_Spd0) < 100.0 || fabs(momwheel.mwhl3.whlspdnow + MomWheel_Spd0) < 100.0)
    {
        flagz = 0;
    }
    if(flagz == 1)
    {
        if(momwheel.mwhl3.whlspdnow < 0)
        {
            errortq = momwheel.mwhl3.whlinertia*(-MomWheel_Spd0 - momwheel.mwhl3.whlspdnow) / deltaT;
        }
        else
        {
            errortq = momwheel.mwhl3.whlinertia*(MomWheel_Spd0 - momwheel.mwhl3.whlspdnow) / deltaT;
        }
        if(errortq > MomWheel_Tq_Max) errortq = MomWheel_Tq_Max;
        if(errortq < -MomWheel_Tq_Max) errortq = -MomWheel_Tq_Max;
        if(errortq > 0)
        {
            pstime[3] = pstime[6] = errortq;
        }
        else
        {
            pstime[2] = pstime[7] = -errortq;
        }
        momwheel.mwhl3.whltq = errortq;
        detwhlspd = momwheel.mwhl3.whltq*deltaT/momwheel.mwhl3.whlinertia;
        momwheel.mwhl3.whlspddsrd = momwheel.mwhl3.whlspdnow + detwhlspd;
    }
    /*动量轮4卸载*/
    if((momwheel.mwhl4.whlspdnow > MomWheel_Spd_Up) || (momwheel.mwhl4.whlspdnow < -1.0*MomWheel_Spd_Up))
    {
        flag4 = 1;
    }
    if(fabs(momwheel.mwhl4.whlspdnow - MomWheel_Spd0) < 100.0 || fabs(momwheel.mwhl4.whlspdnow + MomWheel_Spd0) < 100.0)
    {
        flag4 = 0;
    }
    if(flag4 == 1)
    {
        if(momwheel.mwhl4.whlspdnow < 0)
        {
            errortq = momwheel.mwhl4.whlinertia*(-MomWheel_Spd0 - momwheel.mwhl4.whlspdnow) / deltaT;
        }
        else
        {
            errortq = momwheel.mwhl4.whlinertia*(MomWheel_Spd0 - momwheel.mwhl4.whlspdnow) / deltaT;
        }
        if(errortq > MomWheel_Tq_Max) errortq = MomWheel_Tq_Max;
        if(errortq < -MomWheel_Tq_Max) errortq = -MomWheel_Tq_Max;
        if(errortq > 0)
        {
            pstime[1] = pstime[4] = 0.57735*errortq;
            pstime[2] = pstime[7] = 0.57735*errortq;
            pstime[8] = 2.0*0.57735*errortq;
        }
        else
        {
            pstime[0] = pstime[5] = -0.57735*errortq;
            pstime[3] = pstime[6] = -0.57735*errortq;
            pstime[9] = -2.0*0.57735*errortq;
        }
        momwheel.mwhl4.whltq = errortq;
        detwhlspd = momwheel.mwhl4.whltq*deltaT/momwheel.mwhl4.whlinertia;
        momwheel.mwhl4.whlspddsrd = momwheel.mwhl4.whlspdnow + detwhlspd;
    }

    /*力矩与喷气时间的对应*/
    for(int i=0; i<10; i++)
    {
        pstime[i] = 2*pstime[i] + 10;   /*在此处调整力矩大小*/
        pstime[i] = ceil(pstime[i]);
    }
}
